#include "ziyan_can_adapter.h"
#include "sensor_utils.hpp"
#include <linux/can.h>
#include <linux/can/raw.h>

using namespace sensor::radar;
using namespace sensor::utils;
ZiyanCanAdapter::ZiyanCanAdapter(std::unordered_map<std::string, std::shared_ptr<CanTransreceiver>> devs)
    : CanAdapter(devs, "Nezha")
{
    using namespace std::placeholders;
    message_unpack_table_ = {

    };
    std::map<uint16_t, unsigned int> canIdMap = {

    };
    // can_devs_["can0"]->filter(canIdMap);
}

std::unordered_map<int, int> ZiyanCanAdapter::ZiyanObjectTypeMap = {
    { 0, 1 }, // 4轮车
    { 1, 102 }, // 2轮车
    { 2, 101 }, // 行人
    { 3, 0 }, // 未知
};

void ZiyanCanAdapter::onReceiveCanMessage(const std::string& dev, int fid, muduo::net::Buffer& buffer)
{
    // RCLCPP_INFO(logger_, "ChuanSu Rx:%s %04x %d %s", dev.c_str(), fid, buffer.readableBytes(),
    //     encodeToHexString(buffer.toStringPiece().as_string()).c_str());

    auto iter = message_unpack_table_.find(fid);
    if (iter == message_unpack_table_.end()) {
        // RCLCPP_WARN(logger_, "No Handle to unpack frame %04x", fid);
        return;
    }

    iter->second(dev, fid, buffer);
}

void ZiyanCanAdapter::onReceiveGnssMessage(const interface::msg::Gnss::SharedPtr)
{
}

void ZiyanCanAdapter::onReceiveInformationMessage(const interface::msg::CarInformation msg)
{
    RCLCPP_INFO(logger_, " ZiyanCanAdapter onReceiveInformationMessage start");
    // 手动档位
    static std::unordered_map<int, int> gearsMkMap = {
        { 0, 0xa },
        { 1, 0x1 },
        { 2, 0x2 },
        { 3, 0x3 },
        { 4, 0x4 },
        { 5, 0x5 },
        { 6, 0x6 },
        { 7, 0xb },
        { 8, 0x2 },
    };
    // 自动档位
    static std::unordered_map<int, int> gearsAutoMkMap = {
        { 0, 0x0 },
        { 1, 0x1 },
        { 2, 0x2 },
        { 3, 0x3 },
        { 4, 0x4 },
        { 5, 0x5 },
        { 6, 0x6 },
        { 7, 0xb },
        { 8, 0xc },
    };
    // {
    //     // 方向盘 0xe0
    //     mc1_fmr_sas_status_t safStatus { 0 };
    //     safStatus.sas_steer_wheel_angle = mc1_fmr_sas_status_sas_steer_wheel_angle_encode(msg.wheelangle);
    //     safStatus.sas_failure_sts = mc1_fmr_sas_status_sas_steer_wheel_angle_encode(1.0);
    //     uint8_t dstp[8];
    //     if (int ret = mc1_fmr_sas_status_pack(dstp, &safStatus, sizeof(dstp))) {
    //         muduo::net::Buffer buffer;
    //         buffer.append(dstp, ret);
    //         canSend("can1", MC1_FMR_SAS_STATUS_FRAME_ID, buffer);
    //     }
    // }
    {
        // 车速 0x137
        b30_001_esp_fd2_t espfd2 { 0 };
        espfd2.veh_spd = b30_001_esp_fd2_veh_spd_encode(msg.speed);
        espfd2.veh_spd_vld = b30_001_esp_fd2_veh_spd_vld_encode(1.0);
        uint8_t dstp[64];
        if (int ret = b30_001_esp_fd2_pack(dstp, &espfd2, sizeof(dstp))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", B30_001_ESP_FD2_FRAME_ID, buffer);
        }
    }
    // {
    //     // 自动档位  0x165
    //     mc1_fmr_tcu_status_t tcuStatus { 0 };
    //     tcuStatus.tcu_current_gear_position = gearsAutoMkMap[msg.gearstate];
    //     uint8_t dstp[8];
    //     if (int ret = mc1_fmr_tcu_status_pack(dstp, &tcuStatus, sizeof(dstp))) {
    //         muduo::net::Buffer buffer;
    //         buffer.append(dstp, ret);
    //         canSend("can1", MC1_FMR_VCU_MODE_CONTROL_FRAME_ID, buffer);
    //     }
    // }
    {
        //     // 偏航角 0x245
        b30_001_abm2_t abm2 { 0 };
        abm2.veh_yaw_rate = msg.yawrate * M_PI / 180.0;
        uint8_t dstp[8];
        if (int ret = b30_001_abm2_pack(dstp, &abm2, sizeof(abm2))) {
            muduo::net::Buffer buffer;
            buffer.append(dstp, ret);
            canSend("can1", B30_001_ABM2_FRAME_ID, buffer);
        }
    }
}